#include "master_manager.h"
#include <shared_parameters/shared_params_interface.hpp>

/***************************************************************
 * @file       ModbusMaster.cpp
 * @brief      Constructor
 * @author     yinweibin
 * @version    v1
 * @return     null
 * @date       2023/11/14
 **************************************************************/
MasterManager::MasterManager(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()):
    Node("Modbus", options)
{
    initParam();
}

MasterManager::~MasterManager()
{
    for (size_t i = 0; i < m_ModbusHandles.size(); i++)
    {
        delete m_ModbusHandles[i];
    }
}

void MasterManager::create_tree_branch(std::string prefix, int max)
{
    std::string temp, temp_prefix;
    RCLCPP_INFO(rclcpp::get_logger("Modbus"), "\n\r\n\r---------------");
    RCLCPP_INFO(rclcpp::get_logger("Modbus"), "[MasterManager]%s create tree,have %d branch",prefix.c_str(), max);
    for(int i = 1; i <= max; i++)
    {
        temp_prefix= prefix + std::to_string(i);
        temp= temp_prefix + "/enable";
        bool enable;  
        this->get_parameter_or(temp, enable, false);
        RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s: %d",temp.c_str(), enable);

        if (!enable) break;

        ModbusCommunication* pModbusInterface = new ModbusSlave(shared_from_this());       
        int ret= pModbusInterface->init(temp_prefix);
        if ( ret >= 0)
        {
            pModbusInterface->start();
            m_ModbusHandles.push_back(pModbusInterface);
            RCLCPP_INFO(rclcpp::get_logger("Modbus"), "[MasterManager]%s has started the slave device polling,result=%d",temp_prefix.c_str(), ret);
        }
        else
        {
            delete pModbusInterface;
            pModbusInterface= nullptr;
            RCLCPP_ERROR(rclcpp::get_logger("Modbus"), "[MasterManager]%s failed start the slave device polling,result= %d",prefix.c_str(), ret);    
        }
    }
}


bool MasterManager::initParam() 
{
    if (!SharedParamsInterface::initialize()) {
        RCLCPP_WARN_STREAM(get_logger(), "Failed to initialize shared memory interface");
        return false;
    }

    try {
        auto params = SharedParamsInterface::getNodeParams("shared_parameters");
        if (params.empty() || (params.find("error_code") != params.end() && params["error_code"].getValue<int>() > 0)) {
          RCLCPP_WARN_STREAM(get_logger(), "Error get shared parameters");
          return false;
        } 

        params = SharedParamsInterface::getNodeParams("deviceTree");
        for (const auto& [name, value] : params) {
           
            RCLCPP_INFO_STREAM(get_logger(), 
                    "param:" << name << ", value:" << value.toString());
            switch(value.type) {
                case SharedParamsInterface::ParamValue::Type::INT:
                    this->declare_parameter(name, value.getValue<int>());
                    break;
                case SharedParamsInterface::ParamValue::Type::DOUBLE:
                    this->declare_parameter(name, value.getValue<double>());
                    break;
                case SharedParamsInterface::ParamValue::Type::STRING:
                    this->declare_parameter(name, value.getValue<std::string>());
                    break;
                case SharedParamsInterface::ParamValue::Type::BOOL:
                    this->declare_parameter(name, value.getValue<bool>());
                    break;
                default:
                    RCLCPP_WARN_STREAM(get_logger(), 
                            "Unsupported parameter type for " << name);
                    break;
            }
        }
    } catch (const std::exception& e) {
        RCLCPP_WARN_STREAM(get_logger(), "Error reading parameters: " << e.what());
        return false;
    }
    return true;
}

/***************************************************************
 * @file       MasterManager.cpp
 * @brief      init
 * @author     yinweibin
 * @version    v1
 * @return     null
 * @date       2023/11/14
 **************************************************************/
void MasterManager::init()
{
    /***************************************************************/
    // step1:查找以太网口设备
    
    int xnet_num;
    this->get_parameter_or("xnet_num", xnet_num, 0);
    create_tree_branch(DEVICETREE_XNET, xnet_num);
    /***************************************************************/
    
    /***************************************************************/
    // step1:遍历RS485串口1设备
    
    int uart_num;
    this->get_parameter_or("uart_num", uart_num, 0);
    create_tree_branch(DEVICETREE_RS485, uart_num);
}

